{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "##### 如何找到一条可达的路径\n",
    "+ 静态障碍物，永久障碍物，不可移动障碍物\n",
    "+ 动态障碍物，其他移动物体在移动过程中造成的空间占用。在时空上可以错开的，通过管控合理避让解决\n",
    "+ 任务型永久障碍物，其他移动智能体的目标位置，\n",
    "##### 运动学分析\n",
    "+ 将车体外轮廓建模为矩形\n",
    "##### 车体碰撞检测方法\n",
    "+ 在全局地图坐标系中判断\n",
    "  通过计算欧拉距离判断是否有足够空间，计算复杂度高\n",
    "+ 在车辆视图坐标系中判断\n",
    "  计算复杂度低，采用该方法\n",
    "\n",
    "##### 性能优化\n",
    "+ 寻找复杂度更优的算法\n",
    "+ 矢量化计算\n",
    "+ 减少for循环"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 258,
   "metadata": {},
   "outputs": [],
   "source": [
    "#车体外轮廓，建模为矩形\n",
    "W = 1.4\n",
    "L = 1\n",
    "# 外轮廓矩形顶点\n",
    "VRX = [W / 2, -W / 2, -W / 2, W / 2, W / 2]\n",
    "VRY = [L / 2, L / 2, -L / 2, -L / 2, L / 2]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 259,
   "metadata": {},
   "outputs": [],
   "source": [
    "from math import sqrt, cos, sin, tan, pi\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "from scipy.spatial.transform import Rotation as Rot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 260,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pi_2_pi(angle):\n",
    "    \"\"\"将[0,2pi] 区间转换为[-pi, pi]\"\"\"\n",
    "    return (angle + pi) % (2 * pi) - pi\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 261,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_arrow(x, y, yaw, length=0.3, width=0.2, fc=\"b\", ec=\"k\"):\n",
    "    \"\"\"Plot arrow.\n",
    "    (x, y) 箭头起点\n",
    "    \"\"\"\n",
    "    if not isinstance(x, float):\n",
    "        for (i_x, i_y, i_yaw) in zip(x, y, yaw):\n",
    "            plot_arrow(i_x, i_y, i_yaw)\n",
    "    else:\n",
    "        plt.arrow(x, y, length * cos(yaw), length * sin(yaw),\n",
    "                  fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "车体外轮廓构成的矩形的四个点，在车体坐标系下，相对位置不变。车体航向角，就是围绕着Z轴转动"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 262,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_outline(x, y, yaw):\n",
    "    yaw = np.deg2rad(yaw)\n",
    "    rot = Rot.from_euler('z', -yaw).as_matrix()[0:2, 0:2]\n",
    "    car_outline_x, car_outline_y = [], []\n",
    "    for rx, ry in zip(VRX, VRY):\n",
    "        converted_xy = np.stack([rx, ry]).T @ rot\n",
    "        car_outline_x.append(converted_xy[0]+x)\n",
    "        car_outline_y.append(converted_xy[1]+y)\n",
    "        print(\"origin:x:{} y:{}    new:x:{} y:{}\".format(rx, ry, converted_xy[0]+x, converted_xy[1]+y))\n",
    "    return car_outline_x, car_outline_y\n",
    "\n",
    "def plot_car(x, y, yaw, outline_x, outline_y):\n",
    "    car_color = '-k'\n",
    "    yaw = np.deg2rad(yaw)\n",
    "    c, s = cos(yaw), sin(yaw)\n",
    "\n",
    "    arrow_x, arrow_y, arrow_yaw = c * 0.1 + x, s * 0.1 + y, yaw\n",
    "    plot_arrow(arrow_x, arrow_y, arrow_yaw)\n",
    "\n",
    "    plt.plot(outline_x, outline_y, car_color)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 263,
   "metadata": {},
   "outputs": [],
   "source": [
    "def rectangle_check(x, y, yaw, ox, oy):\n",
    "    '''\n",
    "    将障碍物转换到车坐标系中\n",
    "    '''\n",
    "    yaw = np.deg2rad(yaw)\n",
    "    rot = Rot.from_euler('z', yaw).as_matrix()[0:2, 0:2]\n",
    "    for iox, ioy in zip(ox, oy):\n",
    "        tx = iox - x\n",
    "        ty = ioy - y\n",
    "        converted_xy = np.stack([tx, ty]).T @ rot\n",
    "        rx, ry = converted_xy[0], converted_xy[1]\n",
    "\n",
    "        if (rx < W/2.0 and rx > -W/2.0 and ry < L / 2.0 and ry > -L / 2.0):\n",
    "            print(\"collision point x:{} y:{}\".format(iox, ioy))\n",
    "            return True  # collision\n",
    "            \n",
    "        \n",
    "\n",
    "    return False  # no collision"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 264,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 障碍物\n",
    "ox = [2, 0.6, -0.4]\n",
    "oy = [3, 0.4, 0.4]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 265,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "origin:x:0.7 y:0.5    new:x:0.7 y:0.5\n",
      "origin:x:-0.7 y:0.5    new:x:-0.7 y:0.5\n",
      "origin:x:-0.7 y:-0.5    new:x:-0.7 y:-0.5\n",
      "origin:x:0.7 y:-0.5    new:x:0.7 y:-0.5\n",
      "origin:x:0.7 y:0.5    new:x:0.7 y:0.5\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# yaw 与x轴的夹角\n",
    "# scenaria 1\n",
    "x, y, yaw = 0., 0., 0.\n",
    "plt.axis('equal')\n",
    "car_outline_x, car_outline_y = get_outline(x, y, yaw)\n",
    "\n",
    "plt.scatter(ox, oy)\n",
    "plot_car(x, y, yaw,car_outline_x, car_outline_y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 267,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "collision point x:0.6 y:0.4\n",
      "空间不够，没法通过\n"
     ]
    }
   ],
   "source": [
    "ret = rectangle_check(x, y, yaw, ox, oy)\n",
    "if ret:\n",
    "    print(\"空间不够，没法通过\")\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 269,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "origin:x:0.7 y:0.5    new:x:-0.5000000000000001 y:0.7000000000000001\n",
      "origin:x:-0.7 y:0.5    new:x:-0.5000000000000001 y:-0.7000000000000001\n",
      "origin:x:-0.7 y:-0.5    new:x:0.5000000000000001 y:-0.7000000000000001\n",
      "origin:x:0.7 y:-0.5    new:x:0.5000000000000001 y:0.7000000000000001\n",
      "origin:x:0.7 y:0.5    new:x:-0.5000000000000001 y:0.7000000000000001\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# yaw 与x轴的夹角\n",
    "# scenaria 2\n",
    "x, y, yaw = 0., 0., 90.\n",
    "plt.axis('equal')\n",
    "car_outline_x, car_outline_y = get_outline(x, y, yaw)\n",
    "\n",
    "plt.scatter(ox, oy)\n",
    "plot_car(x, y, yaw,car_outline_x, car_outline_y)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 270,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "collision point x:-0.4 y:0.4\n",
      "空间不够，没法通过\n"
     ]
    }
   ],
   "source": [
    "for x, y in zip(car_outline_x, car_outline_y):\n",
    "    ret = rectangle_check(x, y, yaw, ox, oy)\n",
    "    if ret:\n",
    "        print(\"空间不够，没法通过\")\n",
    "        break"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 275,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "origin:x:0.7 y:0.5    new:x:2.2 y:3.1\n",
      "origin:x:-0.7 y:0.5    new:x:0.8 y:3.1\n",
      "origin:x:-0.7 y:-0.5    new:x:0.8 y:2.1\n",
      "origin:x:0.7 y:-0.5    new:x:2.2 y:2.1\n",
      "origin:x:0.7 y:0.5    new:x:2.2 y:3.1\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# yaw 与x轴的夹角\n",
    "# scenaria 3\n",
    "# x, y, yaw = 1.5, 2.4, 0.\n",
    "x, y, yaw = 1.5, 2.6, 0.\n",
    "plt.axis('equal')\n",
    "car_outline_x, car_outline_y = get_outline(x, y, yaw)\n",
    "\n",
    "plt.scatter(ox, oy)\n",
    "plot_car(x, y, yaw,car_outline_x, car_outline_y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 276,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "collision point x:2 y:3\n",
      "空间不够，没法通过\n"
     ]
    }
   ],
   "source": [
    "ret = rectangle_check(x, y, yaw, ox, oy)\n",
    "if ret:\n",
    "    print(\"空间不够，没法通过\")\n",
    "        "
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.5 ('base')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "19d1d53a962d236aa061289c2ac16dc8e6d9648c89fe79f459ae9a3493bc67b4"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
